/////////////////////////////////////////
//yulishuai
//v1.0
//实现遥控器，ｃｍｄ＿ｖｅｌ转速控制，左肩键低速，右肩键告诉，ｘ遥控，ａ规划
//待改：ｊｏｙＦｌａｇ的初始值
/////////////////////////////////////////


#include<ros/ros.h>
#include<geometry_msgs/Twist.h>
#include<iostream>
#include "std_msgs/Int8.h"
#include <std_msgs/Float64.h>
#include <sensor_msgs/Joy.h>
#include <std_msgs/Float64MultiArray.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

float WHEEL_L=2.8;
float WHEEL_R=0.6;
float WHEEL_PI=3.141693;
float v1=0,v2=0,v3=0,v4=0,v5=0,v6=0;
float cv1=0,cv2=0,cv3=0,cv4=0,cv5=0,cv6=0;
float speed_k=1;
int joyFlag=0;
double speed_x,speed_y,speed_w;
 

void joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{
 
	if(joy->buttons[5])
		speed_k=5;
	else if(joy->buttons[4])
		speed_k=10;
	
  // take the data from the joystick and manipulate it by scaling it and using independent axes to control the linear and angular velocities of the turtle
if(joy->buttons[2])		//X键：手柄［２］
{
	joyFlag=1;
	ROS_INFO("Received a /joy message!");
	speed_x = joy->axes[1]*speed_k;//v
	speed_y = 0;
	speed_w =joy->axes[0]*speed_k;//w

 	v1 =speed_x-0.5*WHEEL_L*speed_w;   //左边    //转化为每个轮子的线速度
 	v3 =v1;
 	v5 =v1;
   
 	v2 =speed_x+0.5*WHEEL_L*speed_w;
 	v4 =v2;
 	v6 =v2;
    ROS_INFO("v1:  %f\t v2:  %f\tv3:  %f\tv4:  %f\tv5:  %f\tv6:  %f\t ",v1,v2,v3,v4,v5,v6);
 
 	v1 =v1/WHEEL_R;    //转换为轮子的速度　rad/s
  	v2 =v2/WHEEL_R;
  	v3 =v3/WHEEL_R;
  	v4 =v4/WHEEL_R;
 	v5 =v5/WHEEL_R;
 	v6 =v6/WHEEL_R;
 
}
else if(joy->buttons[0])//手柄 A 键
{
	joyFlag=0;
	ROS_INFO("justice joyFlag=0");
} 

}


void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{
  //float speed_x,speed_y,speed_w;
  
  ROS_INFO("Received a /cmd_vel message!");
  speed_x = msg->linear.x;//v
  speed_y = 0;
  speed_w = msg->angular.z;//w
  
   cv1 =speed_x-0.5*WHEEL_L*speed_w;   //左边    //转化为每个轮子的线速度
   cv3 =cv1;
   cv5 =cv1;
   
   cv2 =speed_x+0.5*WHEEL_L*speed_w;
   cv4 =cv2;
   cv6 =cv2;
    ROS_INFO("cv1:  %f\t cv2:  %f\tcv3:  %f\tcv4:  %f\tcv5:  %f\tcv6:  %f\t ",cv1,cv2,cv3,cv4,cv5,cv6);
	
   cv1 =cv1/WHEEL_R;    //转换为轮子的速度　rad/s
   cv2 =cv2/WHEEL_R;
   cv3 =cv3/WHEEL_R;
   cv4 =cv4/WHEEL_R;
   cv5 =cv5/WHEEL_R;
   cv6 =cv6/WHEEL_R;
    
}
 
int main(int argc, char** argv)
{
	
	ros::init(argc, argv, "motive_control");
	ros::NodeHandle n;
	ros::Subscriber tsub = n.subscribe("/joy", 1000, joyCallback);
	ros::Subscriber sub = n.subscribe("/cmd_vel", 1000, cmd_vel_callback);
	ros::Publisher six_wheel_torque_pub = n.advertise<std_msgs::Float64MultiArray>("wheel_torque_publisher", 1000);//发布话题wheel_speed_publisher
	//ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
	
	
	//double x = 0.0;
	//double y = 0.0;
	//double th = 0.0;	
	
	//ros::Time current_time, last_time;
	//current_time = ros::Time::now();
 	//last_time = ros::Time::now();

 	ros::Rate loop_rate(10);

while (ros::ok())
  {
    std_msgs::Float64MultiArray msg;
	if(joyFlag == 1)
	{
		msg.data.push_back(v1);
		msg.data.push_back(v2);
		msg.data.push_back(v3);
		msg.data.push_back(v4);
		msg.data.push_back(v5);
		msg.data.push_back(v6);
		v1=0; v2=0; v3=0; v4=0; v5=0; v6=0;
	}
	else if(joyFlag == 0)
	{
		msg.data.push_back(cv1);
		msg.data.push_back(cv2);
		msg.data.push_back(cv3);
		msg.data.push_back(cv4);
		msg.data.push_back(cv5);
		msg.data.push_back(cv6);
		//joyFlag=2;
		cv1=0; cv2=0; cv3=0; cv4=0; cv5=0; cv6=0;
		//ROS_INFO("joyFlag=0");
	}
	six_wheel_torque_pub.publish(msg);
    ros::spinOnce();
    loop_rate.sleep();
  }
 
  return 0;
}

